#pragma once
#include <Math/Matrix3x3.hpp>
#include <Math/Vector3.hpp>
#include <Math/Vector4.hpp>
#include "../VisionTools.hpp"
#include <Graphics/Rotation.hpp>
#include <Graphics/Translation.hpp>

namespace zzz{
template<typename DataType=double>
class ProjectionMat
{
public:
  ProjectionMat()
  :K_(1,0,0,0,1,0,0,0,1),R_(1,0,0,0,1,0,0,0,1),T_(0,0,0) {}

  void Identical() {
    Dress(P_)=(Diag(Ones<DataType>(3,1)),Zeros<DataType>(3,1));
    R_.Identical();
    T_=0;
  }

  void SetP(const Matrix<3,3,DataType> &M, const Vector<3,DataType> &m) {
    //F=[M|m]
    Dress(P_)=(Dress(M),Dress(m));
  }
  void SetP(const Matrix<3,4,DataType> &P) {
    P_=P;
  }
  void MakeP() {
    //P=K[R|T]
    Dress(P_)=(Dress(K_*R_),Dress(K_*T_));
    if (P_(2,3)!=1) P_/=Abs(P_(2,3));
  }

  void SetRT(const Matrix<3,3,DataType> &R, const Vector<3,DataType> &T) {
    R_=R;
    T_=T;
  }
  void SetRotationTranslation(const Rotation<DataType> &R, const Translation<DataType> &t) {
    SetRT(R.Inverted(), R.Inverted()*(-t));
  }
  void MakeRT() {
    Matrix<3,3,DataType> inv_K=K_.Inverted();
    Dress(R_)=Dress(P_)(Colon(),Colon(0,2));
    Dress(T_)=Dress(P_)(Colon(),3);
    R_=inv_K*R_;
    T_=inv_K*T_;
    // scale to rotation matrix
    DataType scale = 1.0 / R_.Row(0).Len();
    R_ *= scale;
    T_ *= scale;
  }

  void SetK(const Matrix<3,3,DataType> &K) {
    K_=K;
  }
  void SetK(DataType fx, DataType fy, DataType cx, DataType cy) {
    K_.Zero();
    K_(0,0)=fx;
    K_(1,1)=fy;
    K_(0,2)=cx;
    K_(1,2)=cy;
    K_(2,2)=1;
  }
  // Vanishing point, 0:X 1:Y 2:Z
  Vector<3,DataType> VPoint(int x) {
    Vector<3,DataType> res;
    Dress(res)=Dress(P_)(Colon(),x);
    return res;
  }

  // Camera center
  // Implementation of table 6.1
  Vector<3,DataType> CameraCenter() const {
    Matrix<3,3,DataType> M;
    Dress(M)=Dress(P_)(Colon(),Colon(0,2));
    Vector<3,DataType> p4;
    Dress(p4)=Dress(P_)(Colon(),3);
    return -(M.Inverted()*p4);
  }
  Vector<3,DataType> PrincipalPoint() const {
    Matrix<3,3,DataType> M;
    Dress(M)=Dress(P_)(Colon(),Colon(0,2));
    return M*M.Row(2);
  }
  Vector<3,DataType> PrincipalRay() const {
    Matrix<3,3,DataType> M;
    Dress(M)=Dress(P_)(Colon(),Colon(0,2));
    return (M.Determinant()*M.Row(2)).Normalized();
  }
  Vector<4,DataType> PrincipalPlane() const {
    return P_.Row(2);
  }

  // rotation and translation
  Rotation<DataType> GetRotation() {
    return Rotation<DataType>(R_.Inverted());
  }
  Translation<DataType> GetTranslation() {
    return Translation<DataType>(-(R_.Inverted() * T_));
  }

  const Matrix<3,4,DataType>& P() const {return P_;}
  const Matrix<3,3,DataType>& R() const {return R_;}
  const Vector<3,DataType>& T() const {return T_;}
  const Matrix<3,3,DataType>& K() const {return K_;}

  const Vector<3,DataType> operator*(const Vector<4,DataType> &x) {
    return P_*x;
  }
  const Vector<2,DataType> operator*(const Vector<3,DataType> &x) {
    return FromHomogeneous(P_*ToHomogeneous(x));
  }
private:
  Matrix<3,4,DataType> P_;
  Matrix<3,3,DataType> R_;
  Vector<3,DataType> T_;
  Matrix<3,3,DataType> K_;
};

typedef ProjectionMat<double> ProjectionMatd;
typedef ProjectionMat<float> ProjectionMatf;
}

